#include "../include/Map.hpp"
namespace ORBSLAM
{

    Map::Map(/* args */)
    {
        mn_max_keyframe_id = 0;
        mb_is_in_use = false;
        mb_bad = false;
    }

    Map::~Map()
    {
    }

    Map::Map(int init_id)
    {
        mn_max_keyframe_id = init_id;
        mn_iniKF_id = init_id;
        mb_is_in_use = false;
        mb_bad = false;
    }

    void Map::Add_key_frame(KeyFrame *pKF)
    {
        unique_lock<mutex> lock(mmutex_map);
        if (msp_keyframe.empty())
        {
            mn_iniKF_id = pKF->mn_keyframe_id;
            mp_iniKF = pKF;
            mp_lower_keyframe = pKF;
        }
        msp_keyframe.insert(pKF);
        if (pKF->mn_keyframe_id > mn_max_keyframe_id)
        {
            mn_max_keyframe_id = pKF->mn_keyframe_id;
        }
        if (pKF->mn_keyframe_id < mp_lower_keyframe->mn_keyframe_id)
        {
            mp_lower_keyframe = pKF;
        }
    }

    void Map::Add_mappoint(MapPoint *pmp)
    {
        unique_lock<mutex> lock(mmutex_map);
        msp_mappoint.insert(pmp);
    }

    int Map::Get_init_KFid()
    {
        unique_lock<mutex> lock(mmutex_map);
        return mn_iniKF_id;
    }

    long unsigned int Map::Get_max_keyframeid()
    {
        unique_lock<mutex> lock(mmutex_map);
        return mn_max_keyframe_id;
    }

    void Map::Set_stored_map()
    {
        mb_is_in_use = false;
    }

    void Map::Set_current_map()
    {
        mb_is_in_use = true;
    }

    void Map::Erase_mappoint(MapPoint *p_mpt)
    {
        unique_lock<mutex> lock(mmutex_map);
        msp_mappoint.erase(p_mpt);
    }

    void Map::Increase_change_index()
    {
        unique_lock<mutex> lock(mmutex_map);
        mn_mapchange++;
    }

    void Map::Erase_keyframe(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_map);
        msp_keyframe.erase(p_keyframe);
        if (msp_keyframe.size() > 0)
        {
            if (p_keyframe->mn_keyframe_id == mp_lower_keyframe->mn_keyframe_id)
            {
                vector<KeyFrame *> vp_keyframe = vector<KeyFrame *>(msp_keyframe.begin(), msp_keyframe.end());
                sort(vp_keyframe.begin(), vp_keyframe.end(), KeyFrame::Lid);
                mp_lower_keyframe = vp_keyframe[0];
            }
        }
        else
        {
            mp_lower_keyframe = 0;
        }
    }

    long unsigned int Map::Keyframes_in_map()
    {
        unique_lock<mutex> lock(mmutex_map);
        return msp_keyframe.size();
    }

    bool Map::Is_bad()
    {
        return mb_bad;
    }
    
    void Map::Inform_new_big_change()
    {
        unique_lock<mutex> lock(mmutex_map);
        mn_big_change_idx++;
    }

    KeyFrame *Map::Get_orignKF()
    {
        unique_lock<mutex> lock(mmutex_map);
        return mp_iniKF;
    }

    vector<KeyFrame *> Map::Get_all_keyframe()
    {
        unique_lock<mutex> lock(mmutex_map);
        return vector<KeyFrame *>(msp_keyframe.begin(), msp_keyframe.end());
    }

    bool compare_id(KeyFrame * p_keyframe1,KeyFrame* p_keyframe2){
        return p_keyframe1->mn_keyframe_id<p_keyframe2->mn_keyframe_id;
    }

    vector<KeyFrame*> Map::Get_all_keyframe_ordered(){
        unique_lock<mutex> lock(mmutex_map);
        vector<KeyFrame*> vp_all_keyframe = vector<KeyFrame*>(msp_keyframe.begin(), msp_keyframe.end());
        sort(vp_all_keyframe.begin(),vp_all_keyframe.end(),compare_id);
        return vp_all_keyframe;
    }

    vector<MapPoint *> Map::Get_all_mappoint()
    {
        unique_lock<mutex> lock(mmutex_map);
        return vector<MapPoint *>(msp_mappoint.begin(), msp_mappoint.end());
    }

}